#include "xmumonitor.h"

xmumonitor::xmumonitor( char* name, QWidget *parent, Qt::WFlags flags )
	: QMainWindow(parent, flags)
{
	ui.setupUi(this);
	
	m_renderwidget = new RenderWidget( ui.RenderArea );
	setFixedSize(1011,580);

	ShowTime( 0.0 );
	ShowLeftTeamInfo();
	ShowRightTeamInfo();
	ShowObjInfo();
	
	Connect_SLot();
	
	ReadLog( name );
}
xmumonitor::~xmumonitor()
{
	delete m_renderwidget;
}
void xmumonitor::ReadLog( char* name )
{
	m_renderwidget->ParseLog( std::string(name) );
	m_renderwidget->SetCurrentWM( 0 );
	ui.TimeScrollBar->setRange( 0, m_renderwidget->WMSize() );
}
void xmumonitor::Connect_SLot()
{
	QObject::connect( ui.TimeScrollBar,  SIGNAL(valueChanged(int)), this, SLOT(SetCurrentWM(int)) );
	QObject::connect( m_renderwidget,  SIGNAL(CurrentWM(int)), ui.TimeScrollBar, SLOT(setValue(int)) );
}
void xmumonitor::keyPressEvent(QKeyEvent *evt)
{
	if( evt->key() == Qt::Key_Escape )
	{
		close();
	}
	else
	{
		m_renderwidget->keyPressEvent(evt);
	}
}
void xmumonitor::keyReleaseEvent(QKeyEvent *evt)
{
	m_renderwidget->keyReleaseEvent(evt);
}
void xmumonitor::SetCurrentWM( int idx )
{
	m_renderwidget->SetCurrentWM(idx);

	ShowTime( m_renderwidget->GetTime() );
	ShowLeftTeamInfo( m_renderwidget->NameLeftTeam(), m_renderwidget->ScoreTeamLeft() );
	ShowRightTeamInfo( m_renderwidget->NameRightTeam(), m_renderwidget->ScoreTeamRight() );
	ShowObjInfo();
}
void xmumonitor::ShowTime( float t )
{
	QString msg;
	msg.sprintf( "<center><h4>Time: %.2f</h4></center>",t );
	ui.GameTimeLabel->setText(msg);
}
void xmumonitor::ShowLeftTeamInfo( const std::string &name, float score )
{
	QString msg;
	msg.sprintf( "<center><h4>%s  %d</h4></center>", name.data(), (int)score );
	ui.NameLeftTeamLabel->setText(msg);
}
void xmumonitor::ShowRightTeamInfo( const std::string &name, float score )
{
	QString msg;
	msg.sprintf( "<center><h4>%s  %d</h4></center>", name.data(), (int)score );
	ui.NameRightTeamLabel->setText(msg);
}
void xmumonitor::ShowObjInfo()
{
	ui.ObjInfoTextEdit->clear();
 	if( m_renderwidget->m_ObjPos.find("ball") != m_renderwidget->m_ObjPos.end() )
 	{
 		RenderWidget::ObjPos ballPos = m_renderwidget->m_ObjPos["ball"];
 
 		QString msg;
		//msg.sprintf( "<h5>Ball:   %.2f %.2f %.2f</h5>", ballPos.pos[12], ballPos.pos[13], ballPos.pos[14] );
		msg.sprintf( "<h5>Ball:   %.2f %.2f</h5>", ballPos.pos[12], ballPos.pos[13] );
		ui.ObjInfoTextEdit->append(msg);
 		//ui.BallPosLabel->setText(msg);
 	}

	std::map<std::string,RenderWidget::ObjPos>::iterator objIt;
	for( objIt = m_renderwidget->m_ObjPos.begin(); 
		objIt != m_renderwidget->m_ObjPos.end(); objIt ++ )
	{
		if( (*objIt).first == "ball" )
		{
			continue;
		}

		QString msg;
		std::string agent_name = (*objIt).first;
		Angle body_angle = ComputeObjAngle( (*objIt).second );
		if( agent_name.find("Left") != std::string::npos )
		{
			body_angle += 270.0f;
		}
		else if( agent_name.find("Right") != std::string::npos )
		{
			body_angle += 90.0f;
		}
		msg.sprintf( "<h5>%s:  %.2f %.2f %.2f</h5>", 
			agent_name.data(), (*objIt).second.pos[12], (*objIt).second.pos[13], body_angle );
		ui.ObjInfoTextEdit->append(msg);
	}
}
Angle xmumonitor::ComputeObjAngle( const RenderWidget::ObjPos &objPos )
{
	Matrix unit_vector_1(1,4);
	Matrix unit_vector_2(4,1);
	Matrix transM(4,4);
	Matrix res_vector_1;
	Matrix res_vector_2;

	unit_vector_1.SetElement( 0, 0, 1.0f );
	unit_vector_2.SetElement( 0, 0, 1.0f );
	
	for( int i = 0; i < 4; i ++ )
	{
		for( int j = 0; j < 4; j ++ )
		{
			transM.SetElement( i, j, objPos.pos[4*i+j] );
		}
	}

	res_vector_1 = unit_vector_1 * transM;
	res_vector_2 = transM * unit_vector_2;
//  	std::cout<< "res_vector_1\n";
//  	res_vector_1.Print();
//  
//  	std::cout<< "res_vector_2\n";
//  	res_vector_2.Print();
;
// res_vector_1.Print();
// res_vector_2.Print();
// std::cout<< "done======\n";
// transM.Print();
 	double x = res_vector_1.GetElement(0,0);
 	double y = res_vector_1.GetElement(0,1);
 	double z = res_vector_1.GetElement(0,2);
//	std::cout<< " " << x << " " << y << " " << z << "\n";	
//  	double x = res_vector_2.GetElement(0,0);
//  	double y = res_vector_2.GetElement(1,0);
//  	double z = res_vector_2.GetElement(2,0);
// 	std::cout<< "- " << atan2Deg( x, y ) << " " << atan2Deg( y, z ) << " " << atan2Deg( x, z ) << "\n";
	Angle angle = atan2Deg( x, y );
// 	if( angle < 0.0f )
// 	{
// 		angle += 180.0f;
// 	}
// 	else
// 	{
// 		angle = 360.0f - angle;
// 	}
	return angle;
}